#include <iostream>
#include <stdlib.h>
#include <time.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>

//#include <boost/program_options.hpp>
#include <boost/timer.hpp>
#include <boost/format.hpp>
#include <boost/thread.hpp>

#include <Shape/PointCloud.h>
#include <Space/SparseGrid.h>
#include <Space/SparseGridOutOfCore.h>
#include <Space/SparseGridNMMFs.h>
#include <Segmentation/SegmentationGraph.h>
#include <System/Platform.h>

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_cloud_generic (new pcl::PointCloud<pcl::PointXYZRGB>);
SparseGrid * sGrid;
PointCloud * pCloud;
PointCloud * pCloud_0;

void GeneratePCWithRandomColoursPerPoint() { 
  srand(time(NULL));
  for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = sGrid->GetMap()->begin(); cellIterator != sGrid->GetMap()->end(); ++cellIterator) {
		uint8_t r(rand() % 255), g(rand() % 255), b(rand() % 255);
		uint32_t rgb = (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
		for (std::vector<size_t>::iterator pointIterator = cellIterator->second->begin(); pointIterator != cellIterator->second->end(); ++ pointIterator) {
			pcl::PointXYZRGB coloured_point;
			coloured_point.x = pCloud->GetPointAt(*pointIterator)->position.X;
			coloured_point.y = pCloud->GetPointAt(*pointIterator)->position.Y;
			coloured_point.z = pCloud->GetPointAt(*pointIterator)->position.Z;
			coloured_point.rgb = *reinterpret_cast<float*>(&rgb); // type unknown
			pcd_cloud_generic->push_back(coloured_point);				
		}
  }
}

//Callback function for keyboard events
unsigned int text_id = 0;
void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event,
							void* viewer_void)
{
  //boost::shared_ptr<pcl::visualization::PCLVisualizer> v = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer>*> (viewer_void);  
  if (event.getKeySym () == "r" && event.keyDown ())
  {
	std::cout << "r was pressed => replacing point cloud" << std::endl;
	viewer->removeAllPointClouds();
	GeneratePCWithRandomColoursPerPoint();
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pcd_cloud_generic);
	viewer->addPointCloud<pcl::PointXYZRGB> (pcd_cloud_generic, rgb, boost::str(boost::format("PC%d") % 0), 0);	    
  }
}

//void* viewer_void)
void mouseEventOccurred (const pcl::visualization::MouseEvent &event,
						 void* s1)                      
{
	//std::cout << "CBhex=" << std::hex << s1 << std::dec << std::endl;	
	//std::cout << "CBcast=" << (boost::shared_ptr<pcl::visualization::PCLVisualizer>*)s1 << std::endl;
	//std::cout << "CBhexViewer=" << std::hex << &viewer << std::dec << std::endl;	
	//boost::shared_ptr<pcl::visualization::PCLVisualizer> v = *((boost::shared_ptr<pcl::visualization::PCLVisualizer>*) s1);
	
	if (event.getButton () == pcl::visualization::MouseEvent::LeftButton &&
	  event.getType () == pcl::visualization::MouseEvent::MouseButtonRelease)
	{
		std::cout << "Left mouse button released at position (" << event.getX () << ", " << event.getY () << ")" << std::endl;

		char str[512];
		sprintf (str, "text#%03d", text_id ++);
		viewer->addText ("clicked here", event.getX (), event.getY (), str);
	}
}

//Create, set parameters and return new viewer.
boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud, SparseGrid * pSG) {
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	viewer->setBackgroundColor (0, 0, 0);	
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
	viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "pyramid");	
	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "pyramid");
	viewer->addCoordinateSystem (1.0);	
	return (viewer);
}

//Create, set parameters and return new viewer with multiple viewports. 
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewportsVis (std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> *clouds, SparseGrid * pSG) {	
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	int v(0);
	double viewWidth = (double)1/(clouds->size());
	double nextXmin = 0.0;
	double nextXmax = nextXmin + viewWidth;
	//std::cout<<"Setting "<<clouds->size()<< " views with width = " << viewWidth << std::endl;
	for (int i = 0; i<clouds->size(); ++i) {		
		viewer->createViewPort(nextXmin, 0.0, nextXmax , 1.0, v);		
		viewer->setBackgroundColor (0, 0, 0, v);		
		viewer->addText(boost::str(boost::format("viewport%d") % v), 10, 10, boost::str(boost::format("viewport%d") % v), v);		
		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(clouds->at(i));
		viewer->addPointCloud<pcl::PointXYZRGB> (clouds->at(i), rgb, boost::str(boost::format("PC%d") % v), v);	
		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, boost::str(boost::format("PC%d") % v), v);
		//viewer->addCoordinateSystem (1.0);
		//std::cout << "SetHex==" << std::hex << &viewer << std::dec << std::endl;
		//std::cout << "SetCast==" << (void*)&viewer << std::endl;
		//viewer->registerMouseCallback (mouseEventOccurred, (void*)&viewer);
		//viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);        		
		nextXmin = nextXmax;
		nextXmax += viewWidth;		
	}
	viewer->initCameraParameters();
	return (viewer);
}

//Create, set parameters and return new viewer.
boost::shared_ptr<pcl::visualization::PCLVisualizer> boxesVis (Vector3 p_quantities) {
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	viewer->setBackgroundColor (0, 0, 0);	
	pcl::ModelCoefficients coefficients;
	for (int i = 0; i<p_quantities.X; ++i) {
		for (int j = 0; j<p_quantities.Y; ++j) {
			for (int k = 0; k<p_quantities.Z; ++k) {
				coefficients.values.clear();
				coefficients.values.push_back(i);
				coefficients.values.push_back(j);
				coefficients.values.push_back(k);
				coefficients.values.push_back(0);
				coefficients.values.push_back(0); 
				coefficients.values.push_back(0);
				coefficients.values.push_back(1);
				coefficients.values.push_back(0.2f);
				coefficients.values.push_back(0.2f);
				coefficients.values.push_back(0.2f);
				viewer->addCube(coefficients, boost::str(boost::format("%d:%d:%d") % i % j % k));			
			}
		}
	}
	
	viewer->addCoordinateSystem (1.0);
	viewer->initCameraParameters ();
	return (viewer);
}

int main(int argv, char **args)
{
	/*boost::program_options::options_description description("Allowed Settings");

	description.add_options()
		("help", "show this message")
		("mem_upperbound", boost::program_options::value<int>(), "mem upperbound in number of points")
		("pcloud_filepath", boost::program_options::value<int>(), "point cloud to work with")
		("k_crawl", boost::program_options::value<int>(), "k neighbours for crawling")
		("k_type", boost::program_options::value<int>(), "k neighbours for setting point type")
		("curvature", boost::program_options::value<int>(), "curvature for setting point type")
		("kdtree_threshold", boost::program_options::value<int>(), "use kdtree search if number of point in voxel space is bigger")
		;
	*/

	std::string file_path;
	int mub = 0;
	int kNN = 0; 

	if ( argv != 4 ) 
	{
		cout<<"usage: <filename> <mem upperbound> <k>\n";
		return 0;
	}
	else 
	{
		mub = atoi(args[2]);
		kNN = atoi(args[3]);
		ifstream the_file ( args[1] );
		if ( !the_file.is_open() )
			cout<<"Could not open file\n";
		else 
		{
			the_file.close();
			file_path = args[1];
		}
	}    
  

	int numberOfProcs = omp_get_num_procs();
	omp_set_num_threads(numberOfProcs);
	boost::timer timer;	
	//SparseGrid * sGrid;	
	SegmentationGraph * sGraph;
	SegmentationGraph * sGraph2;
	//RanSacManager * rManager;

	//Initialise paths to load from ... eventually will read from argv  
	std::vector<std::string> * file_paths = new std::vector<std::string>();
	//file_paths->push_back(file_path);
	//file_paths->push_back("../../../../Media/PointClouds/PCD/conference_rl_bin.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/RMJ9.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/sibenik_bin.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/rest.pcd");	
	//file_paths->push_back("../../../../Media/PointClouds/PCD/hagar_2_1_bin.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/hagar_7.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/obelisk_bin.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/SouthAfrica40Million_bin.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/SouthAfrica40MillionP1_bin.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/SouthAfrica40MillionP2_bin.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/SouthAfrica40MillionP2.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/mnajdra_bin.pcd");	
	//file_paths->push_back("../../../../Media/PointClouds/PCD/step_part3.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/step_part4.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/big_floor0_02_bin.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/step.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/step0_09.pcd");
	//file_paths->push_back("../../../../Media/PointClouds/PCD/floor0_03.pcd");

	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> * pcdClouds = new std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>();    	
	//pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

	//load files (1 or more) from pcdClouds in one PCS::Core::PointCloud
	//for (int i = 0; i<file_paths->size(); ++i) {	
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_cloud (new pcl::PointCloud<pcl::PointXYZ>);
	if (file_paths->size() > 0) {
		if (pcl::io::loadPCDFile<pcl::PointXYZ> (file_paths->at(0), *pcd_cloud) != -1) {
			std::cout << file_paths->at(0) << "[Sandbox] Loaded " << pcd_cloud->size()  << " points." << std::endl;
			//uncomment once in order to save the currently loaded ascii pcd into binary format. Then load binary equivalent
			//pcl::io::savePCDFileBinary<pcl::PointXYZ> ("../../../../Media/PointClouds/PCD/big_floor0_02_bin.pcd", *pcd_cloud);
			pcdClouds->push_back(pcd_cloud);
			//kdtree.setInputCloud(pcd_cloud);
		}
	}
	//}
		/*int K = 16;
		numberOfProcs = omp_get_num_procs();	
		int idx = 0;
		#pragma omp parallel for schedule(guided)		
		for (int i=0; i<numberOfProcs; i++) {
			std::vector<int> pointIdxNKNSearch(K);
			std::vector<float> pointNKNSquaredDistance(K);
			int tIdx = 0;
			while (idx < pcd_cloud->size()) 
			{
				tIdx = _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&idx), 1);				
				kdtree.nearestKSearch (tIdx, K, pointIdxNKNSearch, pointNKNSquaredDistance);
				if (i % 250000 == 0) 
				{ 
					std::cout << "Processed -> " << idx << " - " << timer.elapsed() << std::endl;				    
				}
				pointIdxNKNSearch.clear();
				pointNKNSquaredDistance.clear();			
			}		
		}
		*/

	//pCloud_0 = new PointCloud(pcdClouds, "Test");
	//pCloud_0->WriteFromPCDASCIIToPCSBinFormat("../../../../Media/PointClouds/PCD/sibenik_bin.pcs");
	//delete pCloud_0;
	//pcdClouds->clear();
	//pcd_cloud.reset();
	
	pCloud = new PointCloud(file_path, mub);
	std::cout << "[Sandbox] Point Cloud Initialised with " << pCloud->GetSize() << " points." << std::endl;
	pCloud->UpSample(2);
	
	//sGrid = new SparseGrid(new Vector3(0.14f, 0.18f, 0.04f));	
	//sGrid->Initialise(pCloud, 0.2f);	
	
	SparseGridNMMFs * sGridNMMF = new SparseGridNMMFs(new Vector3(0.2f, 0.2f, 0.2f));
	sGridNMMF->Initialise(pCloud, 0.2f, mub, kNN);	

	//SparseGridOutOfCore * sGridOOC = new SparseGridOutOfCore(new Vector3(0.14f, 0.18f, 0.04f));
	//sGridOOC->Initialise(pCloud, 0.2f);
	
	//sGraph = new SegmentationGraph(sGrid);
	//sGraph->Build();
	//rManager = new RansacManager(sGrid);
	//rManager->Search();
	
  std::cout << boost::str(boost::format("[Sandbox] All done in %ds") % timer.elapsed()) << std::endl;


  //***************************************************************************
  //Create pcd clouds for visualisation
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_cloud_w_cell_colouring (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_cloud_w_patch_colouring (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_cloud_w_type_colouring (new pcl::PointCloud<pcl::PointXYZRGB>);  
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_cloud_w_same_colouring (new pcl::PointCloud<pcl::PointXYZRGB>);  
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_cloud_w_density_colouring (new pcl::PointCloud<pcl::PointXYZRGB>);
  Point * crtPoint;
/*
#pragma region Calculate a heat map over the sparse grid structure. Red(Dense) <-> Blue(Sparse)
  srand(time(NULL));
  float binDensitySpan = sGrid->GetMaxBinSize() - sGrid->GetMinBinSize();
  for (std::map<grid_key, std::vector<Point*>*>::iterator cellIterator = sGrid->GetMap()->begin(); cellIterator != sGrid->GetMap()->end(); ++cellIterator) {
	  //float d = ((float)cellIterator->second->size() / binDensitySpan);
	  uint8_t r(1), g((cellIterator->second->size() / binDensitySpan) * 255), b(128);    // (255 - (cellIterator->second->size() / binDensitySpan) * 255) ;
	  //std::cout<<"CellSize="<<cellIterator->second->size()<<" Normalised="<<d<<" r="<<(cellIterator->second->size() / binDensitySpan) * 255<<" b="<<255 - (cellIterator->second->size() / binDensitySpan) * 255<<std::endl;
		uint32_t rgb = (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
		for (std::vector<Point*>::iterator pointIterator = cellIterator->second->begin(); pointIterator != cellIterator->second->end(); ++ pointIterator) {
			pcl::PointXYZRGB coloured_point;
			coloured_point.x = (*pointIterator)->position.X;
			coloured_point.y = (*pointIterator)->position.Y;
			coloured_point.z = (*pointIterator)->position.Z;
			coloured_point.rgb = *reinterpret_cast<float*>(&rgb); // type unknown
			pcd_cloud_w_density_colouring->push_back(coloured_point);				
		}
  }
#pragma endregion

#pragma region Assign a different colour (randomly) to points in every cell of the grid
  srand(time(NULL));
  for (std::map<grid_key, std::vector<Point*>*>::iterator cellIterator = sGrid->GetMap()->begin(); cellIterator != sGrid->GetMap()->end(); ++cellIterator) {
		uint8_t r(rand() % 255), g(rand() % 255), b(rand() % 255);
		uint32_t rgb = (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
		for (std::vector<Point*>::iterator pointIterator = cellIterator->second->begin(); pointIterator != cellIterator->second->end(); ++ pointIterator) {
			pcl::PointXYZRGB coloured_point;
			coloured_point.x = (*pointIterator)->position.X;
			coloured_point.y = (*pointIterator)->position.Y;
			coloured_point.z = (*pointIterator)->position.Z;
			coloured_point.rgb = *reinterpret_cast<float*>(&rgb); // type unknown
			pcd_cloud_w_cell_colouring->push_back(coloured_point);				
		}
  }
#pragma endregion
  */
/*#pragma region Assign a different colour (randomly) to each patch in the point cloud
  srand(time(NULL));
  std::map<int, int> patchesCount;
  std::map<int, uint32_t> patches;
  std::map<int, uint32_t>::iterator patchesIterator;    
  for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = sGrid->GetMap()->begin(); cellIterator != sGrid->GetMap()->end(); ++cellIterator) {
		for (std::vector<size_t>::iterator pointIterator = cellIterator->second->begin(); pointIterator != cellIterator->second->end(); ++ pointIterator) {
			crtPoint = pCloud->GetPointAt(*pointIterator);
			if (crtPoint->T == PCS::Core::surface) {
				pcl::PointXYZRGB coloured_point;
				coloured_point.x = crtPoint->position.X;
				coloured_point.y = crtPoint->position.Y;
				coloured_point.z = crtPoint->position.Z;

				if ((patchesIterator = patches.find(crtPoint->pcpId)) == patches.end()) {
					uint8_t r(rand() % 255), g(rand() % 255), b(rand() % 255);
					uint32_t rgb = (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
					patches[crtPoint->pcpId] = rgb;
					patchesCount[crtPoint->pcpId] = 1;
					coloured_point.rgb = *reinterpret_cast<float*>(&rgb);
				} else {
					uint32_t rgb = patchesIterator->second;
					patchesCount[crtPoint->pcpId]++;
					coloured_point.rgb = *reinterpret_cast<float*>(&rgb);
				}
			
				pcd_cloud_w_patch_colouring->push_back(coloured_point);
			}
		}
  }
  /*
  for (std::map<int, int>::iterator patchesCountIterator=patchesCount.begin(); patchesCountIterator!=patchesCount.end(); ++patchesCountIterator) {
	  if (patchesCountIterator->second > 200)
		std::cout << "[Sandbox] PatchID:" << patchesCountIterator->first << " number of points = " << patchesCountIterator->second << std::endl;
  }
  
#pragma endregion
  
#pragma region Assign the same colour to each point in the cloud
  uint8_t p_r(15), p_g(255), p_b(15);
  uint32_t p_rgb = (static_cast<uint32_t>(p_r) << 16 | static_cast<uint32_t>(p_g) << 8 | static_cast<uint32_t>(p_b));
 
  bool alternate = false;
  bool addPoint = true;
  int addedPoints = 0;

	for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = sGrid->GetMap()->begin(); cellIterator != sGrid->GetMap()->end(); ++cellIterator) {
		if (cellIterator->second->size() > 10000) alternate=true;
		for (std::vector<size_t>::iterator pointIterator = cellIterator->second->begin(); pointIterator != cellIterator->second->end(); ++ pointIterator) {
			crtPoint = pCloud->GetPointAt(*pointIterator);
			pcl::PointXYZRGB coloured_point;
			coloured_point.x = crtPoint->position.X;
			coloured_point.y = crtPoint->position.Y;
			coloured_point.z = crtPoint->position.Z;
			coloured_point.rgb = *reinterpret_cast<float*>(&p_rgb);
			
			if (alternate) 
			{
				if (addPoint)
				{
					pcd_cloud_w_same_colouring->push_back(coloured_point); //(show both edge and surface points)
					addedPoints++;
					addPoint = false;
				} else { addPoint = true; }
			} 
			else 
			{
				pcd_cloud_w_same_colouring->push_back(coloured_point); //(show both edge and surface points)
				addedPoints++;
			}
		}		
		alternate=false;
	}
	std::cout << "[Sandbox] Created Plain Point Cloud with " << addedPoints << " points." << std::endl;
#pragma endregion
	*/
	#pragma region Assign the same colour to each point in the cloud
  uint8_t p_r(15), p_g(255), p_b(15);
  uint32_t p_rgb = (static_cast<uint32_t>(p_r) << 16 | static_cast<uint32_t>(p_g) << 8 | static_cast<uint32_t>(p_b));
 
  bool alternate = false;
  bool addPoint = true;
  int addedPoints = 0;

	pCloud->Reset();
	Vector3 crt_point;	
	while (pCloud->GetNext(crt_point)) 
	{			
			if (addedPoints < 10000000) 
			{
				pcl::PointXYZRGB coloured_point;
				coloured_point.x = crt_point.X;
				coloured_point.y = crt_point.Y;
				coloured_point.z = crt_point.Z;
				coloured_point.rgb = *reinterpret_cast<float*>(&p_rgb);
				pcd_cloud_w_same_colouring->push_back(coloured_point); //(show both edge and surface points)
				addedPoints++;
			}
			else 
			{
				break;
			}
	}
	std::cout << "[Sandbox] Created Plain Point Cloud with " << addedPoints << " points." << std::endl;
#pragma endregion

	/*
#pragma region Assign green to surface points and blue to edge points
  uint8_t s_r(15), s_g(255), s_b(15);
  uint8_t e_r(15), e_g(15), e_b(255);
  uint8_t u_r(255), u_g(255), u_b(255);
  uint32_t s_rgb = (static_cast<uint32_t>(s_r) << 16 | static_cast<uint32_t>(s_g) << 8 | static_cast<uint32_t>(s_b));
  uint32_t e_rgb = (static_cast<uint32_t>(e_r) << 16 | static_cast<uint32_t>(e_g) << 8 | static_cast<uint32_t>(e_b));
  uint32_t u_rgb = (static_cast<uint32_t>(u_r) << 16 | static_cast<uint32_t>(u_g) << 8 | static_cast<uint32_t>(u_b));
  size_t surface_points = 0;
  size_t edge_points = 0;
  size_t unknown_points = 0;
  alternate = false;
  bool addEdge = true;
  bool addSurface = true;
  bool addUnknown = true;

	for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = sGrid->GetMap()->begin(); cellIterator != sGrid->GetMap()->end(); ++cellIterator) {
		//if (cellIterator->second->size() > 10000) alternate=true;
		for (std::vector<size_t>::iterator pointIterator = cellIterator->second->begin(); pointIterator != cellIterator->second->end(); ++ pointIterator) {
			crtPoint = pCloud->GetPointAt(*pointIterator);
			pcl::PointXYZRGB coloured_point;
			coloured_point.x = crtPoint->position.X;
			coloured_point.y = crtPoint->position.Y;
			coloured_point.z = crtPoint->position.Z;

			if (crtPoint->T == surface) coloured_point.rgb = *reinterpret_cast<float*>(&s_rgb);
			else if (crtPoint->T == edge) coloured_point.rgb = *reinterpret_cast<float*>(&e_rgb);  //e_rgb
			else coloured_point.rgb = *reinterpret_cast<float*>(&u_rgb); // type unknown  //u_rgb

			if (crtPoint->T == surface)
			{
				if (alternate) 
				{
					if (addSurface)
					{
						pcd_cloud_w_type_colouring->push_back(coloured_point); //(show both edge and surface points)
						surface_points++;
						addSurface = false;
					} else { addSurface = true; }
				} 
				else 
				{
					pcd_cloud_w_type_colouring->push_back(coloured_point); //(show both edge and surface points)
					surface_points++;
				}
			}

			if (crtPoint->T == edge)
			{
				if (alternate)
				{
					if (addEdge)
					{
						pcd_cloud_w_type_colouring->push_back(coloured_point); //(show both edge and surface points)
						edge_points++;
						addEdge=false;
					} else { addEdge = true; }
				}
				else 
				{
					pcd_cloud_w_type_colouring->push_back(coloured_point); //(show both edge and surface points)
					edge_points++;
				}
			}

			if (crtPoint->T == unknown)
			{
				pcd_cloud_w_type_colouring->push_back(coloured_point); //(show both edge and surface points)
				unknown_points++;
			}
		}
		alternate=false;
	}
	std::cout << "[Sandbox] Created Surface/Edge PC with " << surface_points << " SurfPnts,  " << edge_points << " EdgePnts and " << unknown_points << " UnkPnts." << std::endl;
#pragma endregion
	*/

	/*
#pragma region Write the processed point cloud to a temporary pcd file on disk
  srand(time(NULL));
  int cellCount = 0;  
  pcl::PointCloud<pcl::PointXYZHSV>::Ptr pcd_cloud_file (new pcl::PointCloud<pcl::PointXYZHSV>);
  //pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_cloud_file2 (new pcl::PointCloud<pcl::PointXYZ>);
  for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = sGrid->GetMap()->begin(); cellIterator != sGrid->GetMap()->end(); ++cellIterator) {
		for (std::vector<size_t>::iterator pointIterator = cellIterator->second->begin(); pointIterator != cellIterator->second->end(); ++ pointIterator) {
			crtPoint = pCloud->GetPointAt(*pointIterator);
			if (crtPoint->T == PCS::Core::surface) {			
				pcl::PointXYZHSV new_point;				
				new_point.x = crtPoint->position.X;
				new_point.y = crtPoint->position.Y;
				new_point.z = crtPoint->position.Z;								
				if (crtPoint->T == PCS::Core::surface) new_point.h = 0.0f;
				if (crtPoint->T == PCS::Core::edge) new_point.h = 1.0f;
				if (crtPoint->T == PCS::Core::unknown) new_point.h = 2.0f;
				new_point.s = (float) cellCount;
				new_point.v = (float) (crtPoint->pcpId);
				pcd_cloud_file->push_back(new_point);
			}*/
			/*pcl::PointXYZ new_point2;
			new_point2.x = crtPoint->position.X;
			new_point2.x = crtPoint->position.Y;
			new_point2.x = crtPoint->position.Z;
			pcd_cloud_file2->push_back(new_point2);*/
		/*}
		cellCount++;
  }
  //Write file to "c:\Temp\temp.pcd"
  std::cout << "[Sandbox] Writing Binary PCD to c:\\Temp\\PCSTemp\\temp.pcd" << cellCount << std::endl;
  pcl::io::savePCDFileBinary("c:\\Temp\\PCSTemp\\temp.pcd", *pcd_cloud_file);
 
  //Write newly ordered file to disk.
  //std::cout << "[Sandbox] Writing Binary PCD to ../../../../Media/PointClouds/PCD/SouthAfrica40Million_bin.pcd" << cellCount << std::endl;
  //pcl::io::savePCDFileBinary<pcl::PointXYZ> ("../../../../Media/PointClouds/PCD/SouthAfrica40Million_bin.pcd", *pcd_cloud);  
  //pcd_cloud_file2.reset();
  pcd_cloud_file.reset();
#pragma endregion*/
  
  //Release Memory Occupied by Grid
  boost::timer timer_d;
  std::cout << "[Sandbox] Deallocating sGrid Memory ... (";
  delete sGrid;
  std::cout << timer_d.elapsed() << ")" << std::endl;

#pragma region Launch the viewers and GUI 

  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> * clouds = new std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>();
  if (pcd_cloud_w_cell_colouring->size() > 0) clouds->push_back(pcd_cloud_w_cell_colouring);
  if (pcd_cloud_w_same_colouring->size() > 0) clouds->push_back(pcd_cloud_w_same_colouring);
  if (pcd_cloud_w_type_colouring->size() > 0) clouds->push_back(pcd_cloud_w_type_colouring);
  if (pcd_cloud_w_patch_colouring->size() > 0) clouds->push_back(pcd_cloud_w_patch_colouring);
  if (pcd_cloud_w_density_colouring->size() > 0) clouds->push_back(pcd_cloud_w_density_colouring);
  viewer = viewportsVis(clouds, sGrid);    
  while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);		
		boost::this_thread::sleep (boost::posix_time::microseconds (10000));
	}
#pragma endregion 

	//delete temporarily created files
	delete sGridNMMF;

	std::cout << "[PCS Service ... Bye Bye]" << std::endl;
	return 0;
}